#include "include/Grid.hpp"
#include "utils/load_side.hpp"
#include <iostream>
#include <array>

extern int element_shape;
extern int degree_of_basis_function;

Grid::Grid(std::vector<Point>& ps, std::vector<Element>& es)
{
    std::cerr << "initialize class Grid" << std::endl;
    std::map<size_t, std::array<size_t, 3> > query = load_side();
    size_t size_p = ps.size();
    if(element_shape == 3 and degree_of_basis_function == 2)
    {
        std::cerr << "make grid finer for P2 " << std::endl;
        for(auto eiter=es.begin(); eiter<es.end(); eiter++)
        {
            size_t key01 = trans_index_pair(id((*eiter)[0]), id((*eiter)[1]));
            std::array<size_t, 3> value01 = query[key01];
            if(value01[0] == 0)
            {
                double avg_x = ((*eiter)[0][0] + (*eiter)[1][0])/2.0;
                double avg_y = ((*eiter)[0][1] + (*eiter)[1][1])/2.0;
                bool bound = is_boundary((*eiter)[0]) && is_boundary((*eiter)[1]);
                Point np(size_p, {avg_x, avg_y}, bound);
                ps.push_back(np);
                (*eiter).add_point(ps[size_p]);
                query[key01] = {1, size_p, bound};
                size_p += 1;
            }
            else
            {
                if(value01[2] == 1)
                {
                   query[key01][2] = 0;
                   ps[value01[1]].set_marker(0); 
                }
                (*eiter).add_point(ps[value01[1]]);
            }

            size_t key02 = trans_index_pair(id((*eiter)[0]), id((*eiter)[2]));
            std::array<size_t, 3> value02 = query[key02];
            if(value02[0] == 0)
            {
                double avg_x = ((*eiter)[0][0] + (*eiter)[2][0])/2.0;
                double avg_y = ((*eiter)[0][1] + (*eiter)[2][1])/2.0;
                bool bound = is_boundary((*eiter)[0]) && is_boundary((*eiter)[2]);
                Point np(size_p, {avg_x, avg_y}, bound);
                ps.push_back(np);
                (*eiter).add_point(ps[size_p]);
                query[key02] = {1, size_p, bound};
                size_p += 1;
            }
            else
            {
                if(value02[2] == 1)
                {
                   query[key02][2] = 0;
                   ps[value02[1]].set_marker(0); 
                }
                (*eiter).add_point(ps[value02[1]]);
            }
            
            size_t key12 = trans_index_pair(id((*eiter)[1]), id((*eiter)[2]));
            std::array<size_t, 3> value12 = query[key12];
            if(value12[0] == 0)
            {
                double avg_x = ((*eiter)[1][0] + (*eiter)[2][0])/2.0;
                double avg_y = ((*eiter)[1][1] + (*eiter)[2][1])/2.0;
                bool bound = is_boundary((*eiter)[1]) && is_boundary((*eiter)[2]);
                Point np(size_p, {avg_x, avg_y}, bound);
                ps.push_back(np);
                (*eiter).add_point(ps[size_p]);
                query[key12] = {1, size_p, bound};
                size_p += 1;
            }
            else
            {
                if(value12[2] == 1)
                {
                   query[key12][2] = 0;
                   ps[value12[1]].set_marker(0); 
                }
                (*eiter).add_point(ps[value12[1]]);
            }
        }
    }
    vertex = ps;
    elements = es;
    std::cerr << "Grid has " << vertex.size() <<" points and " << elements.size() << " elements in total." << std::endl;
}

